#include "Computor.h"

Computor::Computor()
{
    cv::cv2eigen(cam_to_imu_R1, _cam_to_imu_R1);
    cv::cv2eigen(cam_to_imu_R2, _cam_to_imu_R2);
    cv::cv2eigen(cam_to_imu_T, _cam_to_imu_T);
    cv::cv2eigen(gun_to_imu_T, _gun_to_imu_T);
    cv::cv2eigen(imu_to_inertial_R, _imu_to_inertial_R);
    cv::cv2eigen(inertial_to_imu_R, _inertial_to_imu_R);
    cv::cv2eigen(target_position_inertial, _target_position_inertial);
    // this->clock.begin();
    kalman_init();
    predictor.reset(x_k, A, H, P, Q, R, 0);
}

void Computor::updateArmor(const Detector &detector0)
{
    if (detector0.isLock == false) //识别器没锁定目标，就没读到装甲板
    {
        isRead_armor = false;
        return;
    }
    isRead_armor = true;
    if (detector0.detect_armor.size() != 0)
    {
        target = detector0.target_armor;
    }
    // if (detector0.detect_armor.size() != 0)
    // {
    //     Timer clock0;
    //     ArmorPack armor_pack = {detector0.target_armor, clock0};
    //     ArmorPack *ptr = &armor_pack;
    //     ptr->clock.begin();
    //     armor_queue.push_back(armor_pack);
    // }
    // //清空过期装甲板数据
    // vector<ArmorPack>::iterator itr = armor_queue.begin();
    // while (itr != armor_queue.end())
    // {
    //     itr->clock.end();
    //     if (itr->clock.timeConsuming(SECOND) > 0.25)
    //     {
    //         itr = armor_queue.erase(itr);
    //     }
    //     else
    //     {
    //         itr++;
    //     }
    // }
}

void Computor::updateStatus(DataPack &data)
{

    status = data;
    status.euler_angle[0] = 0;
    status.euler_angle[2] = 0;
    if (status.isRead != true)
    {
        isRead_imu = false;
        cout << "read_imu false" << endl;
        return;
    }
    data.isRead = false;
    isRead_imu = true;
    cout << "imu_pitch=" << status.euler_angle[1] * 180 / PI << endl;
    cout << "imu_Wy=" << status.angular_velocity[1] * 180 / PI << endl;
    //矩阵赋值
    _inertial_to_imu_R(0, 0) = cos(-data.euler_angle[1]) * cos(-data.euler_angle[0]);
    _inertial_to_imu_R(0, 1) = cos(-data.euler_angle[1]) * sin(-data.euler_angle[0]);
    _inertial_to_imu_R(0, 2) = -sin(-data.euler_angle[1]);

    _inertial_to_imu_R(1, 0) = cos(-data.euler_angle[0]) * sin(-data.euler_angle[1]) *
                                   sin(-data.euler_angle[2]) -
                               cos(-data.euler_angle[2]) * sin(-data.euler_angle[0]);
    _inertial_to_imu_R(1, 1) = cos(-data.euler_angle[2]) * cos(-data.euler_angle[0]) +
                               sin(-data.euler_angle[1]) * sin(-data.euler_angle[2]) * sin(-data.euler_angle[0]);
    _inertial_to_imu_R(1, 2) = cos(-data.euler_angle[1]) * sin(-data.euler_angle[2]);

    _inertial_to_imu_R(2, 0) = sin(-data.euler_angle[2]) * sin(-data.euler_angle[0]);
    _inertial_to_imu_R(2, 1) = cos(-data.euler_angle[2]) * sin(-data.euler_angle[1]) *
                                   sin(data.euler_angle[0]) -
                               cos(-data.euler_angle[0]) * sin(-data.euler_angle[2]);
    _inertial_to_imu_R(2, 2) = cos(-data.euler_angle[1]) * cos(-data.euler_angle[2]);

    _imu_to_inertial_R = _inertial_to_imu_R.inverse(); //矩阵求逆
}

void Computor::getTarget()
{
    if (isRead_imu == false)
    {
        cout << "getTarget():computor not read imu" << endl;
    }
    if (isRead_armor == false)
    {
        cout << "getTarget():computor not read armor" << endl;
    }
    if (isRead_armor == false || isRead_imu == false)
    {
        return;
    }
    float positionTemp[3];

    int armor_index = armor_queue.size() - 1;
    positionTemp[0] = target.position_cam[0];
    positionTemp[1] = target.position_cam[1];
    positionTemp[2] = target.position_cam[2];

    // #ifdef ADJUST
    // cout << "position_cam_x=" << positionTemp[0] << "m" << endl;
    // cout << "position_cam_y=" << positionTemp[1] << "m" << endl;
    // cout << "position_cam_z=" << positionTemp[2] << "m" << endl;
    // #endif
    Matrix_T position_cam; //相机坐标系下位置
    position_cam << positionTemp[0], positionTemp[1], positionTemp[2];

    _yaw_cam = atan2(position_cam(2, 0), position_cam(1, 0));
    _pitch_cam = -atan2(-position_cam(0, 0), sqrt(pow(position_cam(1, 0), 2) + pow(position_cam(2, 0), 2)));

    Matrix_T position_imu = _cam_to_imu_R2 * _cam_to_imu_R1 * position_cam;

    position_imu += _cam_to_imu_T;

    //目标在惯性云台坐标系下位置
    _target_position_inertial = _imu_to_inertial_R * position_imu;

    //linshixiufu**************************
    _target_position_inertial(1, 0) = 0 - _target_position_inertial(1, 0);
    _target_position_inertial(2, 0) = 0 - _target_position_inertial(2, 0);
    cout << "_target.x=" << _target_position_inertial(0, 0) << endl;
    cout << "_target.y=" << _target_position_inertial(1, 0) << endl;
    cout << "_target.z=" << _target_position_inertial(2, 0) << endl;
    float angle1;
    angle1 = atan2(_target_position_inertial(1, 0), _target_position_inertial(0, 0)); //arctan(y/x)==atan2(y,x)

    // yaw轴电机旋转角度(须弧度转角度)
    _yaw = angle1 - status.euler_angle[0];

    float horizonal_distence = sqrt(_target_position_inertial(0, 0) * _target_position_inertial(0, 0) + _target_position_inertial(1, 0) * _target_position_inertial(1, 0));

    float vertical_distence = _target_position_inertial(2, 0);

    float angle2;
    angle2 = -atan2(vertical_distence, horizonal_distence);
    newton_init = angle2;
    _pitch = angle2 - status.euler_angle[1];

    cout << "_yaw=" << _yaw * 180.0 / PI << endl;
    cout << "_pitch=" << _pitch * 180.0 / PI << endl;

    cout << "need_yaw=" << _yaw << endl;
    cout << "need_pitch=" << _pitch << endl;
    // //数值计算牛顿迭代法,改好重力下坠再用,先调跟随
    // pitch = NewtonIteration(atan2(vertical_distence,horizonal_distence), k1, g, v, vertical_distence, horizonal_distence);
}

void Computor::predict()
{
}

void Computor::sendData(Serialport &serial0)
{

    static int lost_count = 0;
    float angle[2] = {0.0, 0.0};
    if (_yaw > 0.2 || _yaw < -0.2)
    {
        angle[1] = _yaw * 1.5;
    }
    else
    {
        angle[1] = _yaw * 3;
    }
    if (_pitch > 0.19 || _pitch < -0.19)
    {
        angle[0] = _pitch * 1.5;
    }
    else
    {
        angle[0] = _pitch * 3;
    }

    if (isRead_imu == true && isRead_armor == true)
    {
        lost_count = 0;
        // angle[0]=_yaw;
        // angle[1]=_pitch;

        // if (_yaw < 1 * PI / 180)
        //     angle[1] = 0.0;
        // if (_pitch < 1 * PI / 180)
        //     angle[0] = 0.0;
        cout << "send _yaw=" << _yaw * 180.0 / PI << endl;
        cout << "send_pitch=" << _pitch * 180.0 / PI << endl;
        serial0.sendAngle(angle);
        return;
    }
    else
    {
        lost_count++;
        if (lost_count > 2)
        {
            angle[1] = 0;
            angle[0] = 0;
            cout << "send _yaw=" << 0 << endl;
            cout << "send_pitch=" << 0 << endl;
            serial0.sendAngle(angle);
        }
        else
        {
            serial0.sendAngle(angle);
        }
    }
}

float Computor::NewtonIteration(float init, float k1, float g, float v, float vertical_distence, float horizonal_disntence)
{
    float constnum1;
    float constnum2;
    float constnum3;
    constnum1 = (exp(k1 * horizonal_disntence) - 1) / k1;
    constnum2 = -0.5 * g * pow<float>((exp(k1 * horizonal_disntence) - 1), 2) / pow<float>(k1 * v, 2);
    constnum3 = -vertical_distence;

    float constnum4;
    float constnum5;
    constnum4 = (exp(k1 * horizonal_disntence) - 1) / k1;
    constnum5 = -g * pow<float>((exp(k1 * horizonal_disntence) - 1), 2) / pow<float>(k1 * v, 2);

    float theata_before = init;
    float theata_current = 1;

    while (abs(theata_current - theata_before) >= 0.1 * 180 / PI)
    {
        theata_current = theata_before - originFunction(theata_before, constnum1, constnum2, constnum3) / deriveFunction(theata_before, constnum4, constnum5);
    }
    return theata_current;
}

float Computor::originFunction(float theata_before, float num1, float num2, float num3)
{
    float result;
    result = sin(theata_before) / cos(theata_before) * num1 + 1 / pow<float>(cos(theata_before), 2) * num2 + num3;
    return result;
}

float Computor::deriveFunction(float theata_before, float num1, float num2)
{
    float result;
    result = 1 / pow<float>(cos(theata_before), 2) * num1 + sin(theata_before) / pow<float>(cos(theata_before), 3) * num2;
    return result;
}

void Computor::kalman_init()
{
    x_k = Vector4f::Zero(); //yaw pitch,Wy,Wp
    x_k1 = Vector4f::Zero();
    z_k = Vector4f::Zero(); //yaw,pitch
    A = Matrix4f::Identity();
    H = Matrix4f::Zero();
    Q = Matrix4f::Zero();
    R = Matrix4f::Zero();
    P = Matrix4f::Zero();
    for (int i = 0; i < 2; i++)
    {
        H(i, i) = 1;
        P(i, i) = (float)p0 / 100.0;
    }
    Q(0, 0) = (float)var_q1 / 100.0;
    Q(1, 1) = (float)var_q2 / 100.0;
    R(0, 0) = (float)var_r1 / 100.0;
    R(1, 1) = (float)var_r2 / 100.0;
}

void Computor::outPutStatus()
{
    cout << "angle_yaw_inertial_to_imu=" << status.euler_angle[0] << endl;
    cout << "angle_pitch_inertial_to_imu=" << status.euler_angle[1] << endl;
    cout << "angle_roll_inertial_to_imu=" << status.euler_angle[2] << endl; //理论输出为0
}

void Computor::outPutTargetPosition()
{
    cout << "target_position_inertial_x=" << target_position_inertial.at<float>(0, 0) << endl;
    cout << "target_position_inertial_y=" << target_position_inertial.at<float>(1, 0) << endl;
    cout << "target_position_inertial_z=" << target_position_inertial.at<float>(2, 0) << endl;
}

void Computor::computeTrajectory()
{
    if (isSaveEnd == true)
    {
        cout << "弹道数据收集完毕" << endl;
        return;
    }
    if (isOpen == false)
    {
        angle_pitch.open("../../assert/trajectory/angle_pitch.txt", ios::app | ios::out);
        horizonal.open("../../assert/trajectory/horizonal_distence.txt", ios::app | ios::out);
        vertical.open("../../assert/trajectory/vertical_distence.txt", ios::app | ios::out);
        if (angle_pitch.is_open() && horizonal.is_open() && vertical.is_open())
        {
            cout << "output stream open success" << endl;
            isOpen = true;
        }
        else
        {
            cout << ".txt文件打开失败" << endl;
            return;
        }
    }
    if (waitKey(1) == 'r') // read
    {
        float horizonal_distence = sqrt(pow(target_position_inertial.at<float>(0, 0), 2) + pow(target_position_inertial.at<float>(1, 0), 2));
        float vertical_distence = target_position_inertial.at<float>(2, 0);
        angle_pitch << status.euler_angle[1] << endl;
        horizonal << horizonal_distence << endl;
        vertical << vertical_distence << endl;
    }
    if (waitKey(1) == 's') // save and exit
    {
        angle_pitch.close();
        horizonal.close();
        vertical.close();
        isSaveEnd = true;
        isOpen = false;
    }
}

Computor::~Computor()
{
}
